#!/usr/bin/python3
# coding=utf8
import sys
import json
import time
# import signal
import argparse
# sys.path.append('/home/pi/TurboPi/')
# import HiwonderSDK.mecanum as mecanum

if sys.version_info.major == 2:
    print('Please run this program with python3!')
    sys.exit(0)
    
print('''
**********************************************************
****************功能:小车路径配置************************
**********************************************************
----------------------------------------------------------
''')

# chassis = mecanum.MecanumChassis()

start = True

# 处理程序关闭
def stop(signum, frame):
    global start
    start = False
    print('关闭中...')
    # chassis.set_velocity(0, 0, 0)  # 关闭所有电机

# signal.signal(signal.SIGINT, stop)

def move_car(segments):
    global start
    for segment in segments:
        if not start:
            break

        speed = segment['speed']
        direction = segment['direction']
        time_duration = float(segment['time'])

        # 将方向转换为角度和偏航角速度
        if direction == 'straight':
            angle = 0
            yaw_rate = 0
        elif direction == 'left':
            angle = 90
            yaw_rate = 0.3
        elif direction == 'right':
            angle = 270
            yaw_rate = -0.3
        elif direction == 'u-turn':
            angle = 180
            yaw_rate = 0.6  # 假设掉头的偏航角速度较大
        else:
            print(f"未知方向: {direction}")
            continue

        # 设定转向，如果需要转向
        if direction != 'straight':
            print(f"段 {segment['segment']} 转向: 方向={direction}, 偏航角速度={yaw_rate}")
            # chassis.set_velocity(0, 0, yaw_rate)
            time.sleep(1)  # 转向时间，假设为1秒
            # chassis.set_velocity(0, 0, 0)  # 停止转向

        # 设置前进参数
        print(f"段 {segment['segment']}: 速度={speed}, 方向={angle}, 时间={time_duration}")
        # chassis.set_velocity(speed, angle, 0)  # 设置小车速度和方向

        # 运动时间
        time.sleep(time_duration)

    # chassis.set_velocity(0, 0, 0)  # 关闭所有电机
    print('已关闭')

def main():
    # 解析命令行参数
    parser = argparse.ArgumentParser(description='控制小车的路径配置')
    parser.add_argument('json_path', type=str, help='JSON 文件路径')
    args = parser.parse_args()
    
    # 读取 JSON 文件
    try:
        with open(args.json_path, 'r') as file:
            segments = json.load(file)
    except Exception as e:
        print(f"读取JSON文件时出错: {e}")
        sys.exit(1)
    
    move_car(segments)

if __name__ == '__main__':
    main()
